#include "location.h"
static int g_left_pulse_num = 0;
static int g_right_pulse_num = 0;
static double g_angle_record = 0;
static double g_rec_location_x =0;
static double g_rec_location_y =0;
static u8 if_print_pose = 0;
//记录第一次读到的陀螺仪角度后续以这个为基准传参，因为存在零飘的问题,
static double first_angle_drift = 0;      
#ifdef BATTERY_VERSION

/**
 * @description: location function init
 * @param: void
 * @return: 0 success
 *         -1 fail
 */
int xp_location_init()
{
    int sta = 0;
    sta=xp_wheel_init(2,1,115200);
    if(sta != 0)
    {
       println("xp_wheel_init error -3");
       return -3;
    }
    sta = xp_gyro_init();
	if(0 != sta)
	{
        println("xp_gyro_init fail -1");
		return -1;
	}
    sta = xp_location_encode_clear();
    if(0 != sta)
	{
        println("xp_location_encode_clear");
		return -1;
	}
    aos_msleep(200);
    sta =  xp_wheel_set_wheel_speed(0,0);
    if(sta != 0)
    {
        println("xp_wheel_set_wheel_speed(0,0) fail ");
        return -1;
    }
    first_angle_drift = get_gyro_angle();
    println("first angle is %f ",first_angle_drift);
    println("location init success !!!");
    aos_task_new("xp location thread", xp_location_thread, NULL, 3000);
    return 0;
}
/**
 * @description: get left and ringht wheel driver encode
 * @param: 
 right_pulse:   pointer to the right pulse value
 left_pulse:    pointer to the left pulse value
 * @return: 0 success
 *         -1 fail
 */

int  xp_location_get_pulse_num(int* left_pulse,int* right_pulse)
{
    if(left_pulse == NULL || right_pulse == NULL)
    {
        return -1;
    }
    *left_pulse = xp_wheel_get_left_encode();
    *right_pulse = xp_wheel_get_right_encode();
    return 0;
}

/**
 * @description: location function init
 * @param: void
 * @return: 0 success
 *         -1 fail
 */

void xp_location_thread()
{
    u8 sta = 0;
    double x_coord = 0;
    double y_coord = 0;
    double angle_temf = 0;
    int left_encode = 0;
    int right_encdoe = 0;
    printf("**************************\n");
    printf("xp_location_thread is begining !!!\n");
    printf("**************************\n");
    g_angle_record = xp_lcoation_get_angle();
    sta = xp_location_get_pulse_num(&g_left_pulse_num,&g_right_pulse_num);   
     if(sta != 0)
     {
        println("xp_location_get_pulse_num error");
        
     }
    while(1)
    {
     angle_temf = xp_lcoation_get_angle();
     sta = xp_location_get_pulse_num(&left_encode,&right_encdoe);   
     if(sta != 0)
     {
        println("xp_location_get_pulse_num error");
        continue;
     }
      if(if_print_pose)
        {
        println("g_angle_record %f angle_temf %f ",g_angle_record,angle_temf);
        }
     
     xp_upper_set_robot_pose(g_angle_record,g_rec_location_x,g_rec_location_y);
     g_angle_record = angle_temf; 
     /*
      if(if_print_pose)
			{
     printf("left_encode  %d g_left_pulse_num %d right_encdoe %d g_right_pulse_num %d \n\r  absleft %d  absright %d\n\r",left_encode,
        g_left_pulse_num,right_encdoe,g_right_pulse_num,
        abs(left_encode-g_left_pulse_num),abs(right_encdoe-g_right_pulse_num));
      }
      */
     /*
     if((abs((left_encode-g_left_pulse_num))<20)&&(abs(right_encdoe-g_right_pulse_num)<20))
    {
        if(if_print_pose)
        {
        println("g_angle_record %f angle_temf %f ",g_angle_record,angle_temf);
        }
       if(fabs(g_angle_record-angle_temf)>0.019)
       {
            xp_upper_move_sate_set(7);
            println("<===============xp_upper_move_sate_set(7);=============>");
            aos_msleep(1000);
            g_angle_record =angle_temf;
            continue;
       }
       else if(fabs(g_angle_record-angle_temf)<=0.018)
       {
           xp_upper_move_sate_set(0); 
       }
       aos_msleep(20);
       g_angle_record =angle_temf;
       continue;
     }
     */
     if(xp_wheel_get_enble_state() == 0)
     {
       if((abs((left_encode-g_left_pulse_num))>5000)||(abs(right_encdoe-g_right_pulse_num)>5000))
       {
           xp_upper_move_sate_set(7);
           
           printf("********> xp_upper_move_sate_set(7);******< \n");
       }
       else
       {
           xp_upper_move_sate_set(0);
       }
     }
     sta = xp_calculate_location_coordinate((left_encode-g_left_pulse_num),
     (right_encdoe-g_right_pulse_num),
     angle_temf,
     &x_coord,
     &y_coord
     );
     if(sta != 0)
     {
        aos_msleep(300);
        continue;
     }
     /*
     if(if_print_pose)
     {
        printf("left_encode  %d g_left_pulse_num %d right_encdoe %d g_right_pulse_num %d\n\r",left_encode,
        g_left_pulse_num,right_encdoe,g_right_pulse_num);
     }
     */
     g_left_pulse_num = left_encode;
     g_right_pulse_num = right_encdoe;
     g_rec_location_x = x_coord;
     g_rec_location_y = y_coord;
     if(if_print_pose)
     {
        printf("locate_x %f lcoate_y %f angle_b %f angle_a %f \n",g_rec_location_x
            ,g_rec_location_y,get_gyro_angle(),g_angle_record);
     }
     aos_msleep(300);
    }
}

/**
 * @description: lcoation function get angle from module and  solve the drift problem
 * @param: void
 * @return:yaw angle without drift because filt it
 */
double xp_lcoation_get_angle()
{
    double yaw = 0;
    double anglet = get_gyro_angle() + GREAT_SMALL_POINT_NUMBER;
    if(first_angle_drift>0)
    {
         if(anglet >0 && anglet<180.0)
        {
            yaw = anglet - first_angle_drift;

        }
        else if(anglet<0 && anglet>-180)
        {
            yaw = anglet -first_angle_drift;
            if(yaw < -180.0)
            {
               yaw =360+ yaw;
            }
        }
    }
    else if(first_angle_drift<0)
    {
        if(anglet >0 && anglet<180.0)
        {
            yaw = anglet - first_angle_drift;
            if(yaw >180)
            {
                yaw = yaw -360;
            }

        }
        else if(anglet<0 && anglet>-180)
        {
            yaw = anglet -first_angle_drift;
            
        }    
    }
    return yaw ;
}


/**
 * @description: location function init
 * @param: void
 * @return: 0 success
 *         -1 fail
 */
int  xp_location_encode_clear()
{
     return xp_wheel_set_encode_specifice_value(0);
}

/**
 * @description: location function init
 * @param: void
 * @return: 0 success
 *         -1 fail
 */

int xp_location_debug(char *type,char *fun,char *param)
{
    int ret = 0;
    if(strcmp(type,"location")!=0)
		{
			return 0;
		}
    if(strcmp(fun,"read") == 0)
    {
         printf("locate_x %f lcoate_y %f angle_b %f angle_a %f \n",g_rec_location_x
            ,g_rec_location_y,get_gyro_angle(),g_angle_record);
    }
    else if(strcmp(fun,"oread") == 0)    //关闭打印
    {
        if_print_pose = 1;
    }
    else if(strcmp(fun,"cread") == 0)    //关闭打印
    {
        if_print_pose = 0;
    }
    else if(strcmp(fun,"clear") == 0)
    {
        ret = xp_location_encode_clear();
        if(ret != 0)
        {
            println("lcoation clear fail");
        }
        else
        {
            println("location clear success");
        }
        
    }
    else
    {
        return 0;
    }
    return 1;
    
}

#elif defined LINE_VERSION
#endif


